#include "Servo.h"


/*
*		舵机驱动程序
*				时基脉冲T=20ms
*		0.5ms  -->  0°		duty = 2.5%
*		1ms    -->  45°		duty = 5%
*		1.5ms  -->  90°		duty = 7.5%
*		2ms    -->  135°	duty = 10%
*		2.5ms  -->  180°	duty = 12.5%
*
*		45°/500us = 0.09°/us
*		PWM(duty) = 500us + N°/(0.09°/us)
*/

/**
* 	配置PWM频率：
* 	如果系统时钟频率 = 72Mhz，
* 	配置PWM_SCLK_DIV = 72-1，
* 	配置计数频率 = 0x4E20（20000）
* 	因此，PWM频率 = 72M /（72 * 20000）Hz = 50Hz
*
* 	配置占空比：
* 	配置启动的PWM电平=高电平，则 Duty = CCRx/ARR
* 	配置CCRx= 0x01F4(500)，
* 	因此，占空比= 01F4 / 0x7530 = 2.5％
*	舵机0度的位置时 ARR = 20000*2.5% = 500；180度时 ARR = 20000*12.5% = 2500
*	ARR∈[500,2500] (DEC)细分2500-500=2000份	180°/2000=0.09°每份
*	舵机的死区为3us，则分辨率最低为180°/(20(ms)*1000(us/ms)/3(us)*10%)=0.27°
*	因此每次控制的值应该至少增加0.27/0.09=3舵机才能反应。
**/



/**
 * @brief Servo Init
 * @param hservo Servo handle
 * @param htim TIM handle
 * @param channel TIM channel
 * @param AngleRange Servo angle range
 * @param min Servo max angle
 * @param max Servo min angle
 */
void ServoInit(Servo_t *hservo, TIM_HandleTypeDef *htim, uint8_t channel,
                uint16_t AngleRange, uint16_t min, uint16_t max)
{
	hservo->enable = 0;
    hservo->htim = htim;
    hservo->channel = channel;
    hservo->angleRange = AngleRange;
    hservo->zero = ServoGetPeriod(hservo) * ServoStartAngleDuty + 0.5;
    hservo->minValue = ServoAngle2Value(hservo, min);
    hservo->maxValue = ServoAngle2Value(hservo, max);
    ServoWriteValue(hservo, hservo->minValue);
}

/**
 * @brief Servo Start
 * @param hservo Servo handle
 */
void ServoStart(Servo_t *hservo)
{

    if (hservo->enable == 0)
        ServoEnable(hservo);    
}

/**
 * @brief Servo Stop
 * @param hservo Servo handle
 */
void ServoStop(Servo_t *hservo)
{
    if (hservo->enable)
        ServoDisable(hservo);
}

/**
 * @brief Set Servo angle
 * @param hservo Servo handle
 * @param angle [min,max]
 */
void ServoWrite(Servo_t *hservo, float angle)
{
    uint32_t value;
    value = ServoAngle2Value(hservo, angle);

    if (value > hservo->maxValue)
        value = hservo->maxValue;
    else if (value < hservo->minValue)
        value = hservo->minValue;
    ServoSetCompare(hservo, hservo->zero + value);
}

/**
 * @brief Move servo from current position
 * @param hservo Servo handle
 * @param angle move dir(±) and angle
 */
void ServoMove(Servo_t *hservo, float angle)
{
	ServoWrite(hservo, ServoRead(hservo) + angle);
}

/**
 * @brief Read Servo angle
 * @param hservo Servo handle
 * @retval float currnet angle
 */
float ServoRead(Servo_t *hservo)
{
    return ServoValue2Angle(hservo, ServoGetCompare(hservo) - hservo->zero);
}

/**
 * @brief Set Servo Value
 * @param hservo Servo handle
 * @param value [0,4*zero]
 */
void ServoWriteValue(Servo_t *hservo, uint16_t value)
{
    if (value > hservo->zero*4)
        value = hservo->zero*4;
    ServoSetCompare(hservo, hservo->zero + value);
}

/**
 * @brief Servo move value
 * @param hservo Servo handle
 * @param value move dir(±) and value
 */
void ServoMoveValue(Servo_t *hservo, int value)
{
    ServoWriteValue(hservo, ServoReadValue(hservo) + value);
}

/**
 * @brief Read Servo Value
 * @param hservo Servo handle
 * @retval uint16_t current value
 */
uint16_t ServoReadValue(Servo_t *hservo)
{
    return ServoGetCompare(hservo) - hservo->zero;
}

